#include "TrigoBernsteinSurfPopulation.h"
#include "../Core/Constants.h"

using namespace std;

namespace cagd {
    TrigoBernsteinSurfPopulation::Individual::Individual(GLdouble alpha, GLuint n, GLdouble beta, GLuint m,
               Matrix<DCoordinate3> data_net, RowMatrix<GLdouble> u_knot, ColumnMatrix<GLdouble> v_knot,
               GLuint u_div_point, GLuint v_div_point):
        _alpha(alpha), _n(n), _beta(beta), _m(m),
        _data_net_to_interpolate(data_net),
        _u_knot_vector(u_knot),_v_knot_vector(v_knot),
        _u_div_point_count(u_div_point), _v_div_point_count(v_div_point),
        _individual(new TrigonometricBernsteinSurface3(alpha,n,beta,m))
    {
        _image = 0;
        calculateInterpolation();
//        render();
    }

    TrigoBernsteinSurfPopulation::Individual::Individual(GLdouble alpha, GLuint n, GLdouble beta, GLuint m,
               Matrix<DCoordinate3> data_net,
               GLuint u_div_point, GLuint v_div_point):
        _alpha(alpha), _n(n), _beta(beta), _m(m),
        _data_net_to_interpolate(data_net),
        _u_div_point_count(u_div_point), _v_div_point_count(v_div_point),
        _individual(new TrigonometricBernsteinSurface3(alpha,n,beta,m))
    {
        _image = 0;
        randomizeUKnots(2 * n + 1);
        randomizeVKnots(2 * m + 1);

        calculateInterpolation();
//        render();
    }

    GLboolean TrigoBernsteinSurfPopulation::Individual::calculateInterpolation() {
//        if (!_individual)
//        {
//            throw Exception("Could not create interpolating trigonometric Bernstein like surface!");
//        }
//        else
//        {
            if (!_individual->UpdateDataForInterpolation(_u_knot_vector, _v_knot_vector, _data_net_to_interpolate))
            {
                throw Exception("Could not solve the surface interpolation problem!");
            }
            else
            {
                _fitness = (*_individual).Area(100, 100);
//                cout << "The area of the interpolating surface is " << _fitness << endl;
                if (!_individual->UpdateVertexBufferObjectsOfData())
                {
                    throw Exception("Could not create the vertex buffer object of the interpolating surface's control net!");
                }
            }
//        }
//            render();
        return GL_TRUE;
    }

    GLvoid TrigoBernsteinSurfPopulation::Individual::render() {

//        if (_image) {
//            delete _image, _image = 0;
//        }
//        if (!_individual) {
//            cout << "NINCS MIRE SZAMOLNI!!" << endl;
//        } else {
//            cout << _u_div_point_count << " " << _v_div_point_count << endl;
//        }
        _image = _individual->GenerateImage(_u_div_point_count, _v_div_point_count);

        if (!_image)
        {
            throw Exception("Could not generate the image of the interpolating surface!");
        }
        else
        {
            if (!_image->UpdateVertexBufferObjects())
            {
                throw Exception("Could not create the vertex buffer objects of the interpolating surface's image!");
            }
        }
    }

    GLboolean TrigoBernsteinSurfPopulation::Individual::randomizeUKnots(GLuint number_of_knots) {
        if (!number_of_knots) {
            _u_knot_vector.ResizeColumns(0);
            return GL_FALSE;
        }
        _u_knot_vector.ResizeColumns(number_of_knots);

        GLdouble du = _alpha / (2 * _n);

        for (GLuint i = 0; i < 2 * _n + 1; ++i)
        {
            _u_knot_vector[i] = min(i * du, _alpha);
        }

        for (GLuint i = 1; i < number_of_knots - 2; i++) {
            GLdouble v = (GLdouble)rand() / (GLdouble)RAND_MAX;           // [0 ..   1]
            GLdouble scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);     // [0 ..   1]
            GLdouble r_max = 0.2;
            while (scale > r_max) {                                 // force [0 .. r_max] interval
                scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);    // [0 ..   1]
            }

            if (v < 0.5) {
                _u_knot_vector[i] -= (_u_knot_vector[i] - _u_knot_vector[i-1]) * scale;
            } else {
                _u_knot_vector[i] += (_u_knot_vector[i+1] - _u_knot_vector[i]) * scale;
            }
        }

//        cout << "U Knot vector of Individual: " << _u_knot_vector << endl;

        return GL_TRUE;
    }

    GLboolean TrigoBernsteinSurfPopulation::Individual::randomizeVKnots(GLuint number_of_knots) {
        if (!number_of_knots) {
            _v_knot_vector.ResizeColumns(0);
            return GL_FALSE;
        }

        _v_knot_vector.ResizeRows(2 * _m + 1);

        GLdouble dv = _beta / (2 * _m);

        for (GLuint j = 0; j < 2 * _m + 1; ++j)
        {
            _v_knot_vector[j] = min(j * dv, _beta);
        }

        for (GLuint i = 1; i < number_of_knots - 2; i++) {
            GLdouble v = (GLdouble)rand() / (GLdouble)RAND_MAX;           // [0 ..   1]
            GLdouble scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);     // [0 ..   1]
            GLdouble r_max = 0.2;
            while (scale > r_max) {                                 // force [0 .. r_max] interval
                scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);    // [0 ..   1]
            }

            if (v < 0.5) {
                _v_knot_vector[i] -= (_v_knot_vector[i] - _v_knot_vector[i-1]) * scale;
            } else {
                _v_knot_vector[i] += (_v_knot_vector[i+1] - _v_knot_vector[i]) * scale;
            }
        }
//        cout << "V Knot vector of Individual: " << _v_knot_vector << endl;

        return GL_TRUE;
    }

    GLboolean TrigoBernsteinSurfPopulation::Individual::calculateLength() {
        _fitness = (*_individual).Area(100, 100);
        return GL_TRUE;
    }

    TrigoBernsteinSurfPopulation::TrigoBernsteinSurfPopulation(GLuint number_of_individuals,
                                GLdouble alpha, GLuint n, GLdouble beta, GLuint m,
                                Matrix<DCoordinate3> data_net,
                                GLuint iterationBetweenGeneration,
                                GLuint u_div_point, GLuint v_div_point):
        _alpha(alpha),
        _n(n),
        _beta(beta),
        _m(m),
        _data_net_to_interpolate(data_net),
        _number_of_individuals(number_of_individuals),
        _iterationBetweenGeneration(iterationBetweenGeneration)
    {
        r_max = 0.2;

        if (!number_of_individuals) {
            _individuals.ResizeColumns(0);
        } else {
            _individuals.ResizeColumns(number_of_individuals);

            for (GLuint i = 0; i < number_of_individuals; i++) {
                _individuals[i] = new Individual(alpha, n, beta, m, _data_net_to_interpolate);
            }
            sort();
            for (GLuint i = 0; i < number_of_individuals; i++) {
                cout << "Area of [" << i << "] = " <<  _individuals[i]->getFitness() << endl;
            }

        }
    }

    TrigoBernsteinSurfPopulation::Individual* TrigoBernsteinSurfPopulation::getBest() {
        return _individuals[0];
    }

    RowMatrix<TrigoBernsteinSurfPopulation::Individual*> TrigoBernsteinSurfPopulation::getAllIndividual() {
        return _individuals;
    }

    GLvoid TrigoBernsteinSurfPopulation::sort() {
        Individual* temp;
        GLuint exchange;
        for(GLuint i = 0; i < _number_of_individuals; i++){
            exchange = 0;
            for(GLuint j = 0; j < _number_of_individuals - 1; j++){
                if(_individuals[j]->getFitness() > _individuals[j+1]->getFitness()) {
                    temp = _individuals[j];
                    _individuals[j] = _individuals[j+1];
                    _individuals[j+1] = temp;
                    exchange++;
                }
            }
            if(exchange == 0)
                break;
        }
    }

    RowMatrix<GLdouble> TrigoBernsteinSurfPopulation::sortUKnots(RowMatrix<GLdouble> knots_vector) {
        return knots_vector;
    }

    ColumnMatrix<GLdouble> TrigoBernsteinSurfPopulation::sortVKnots(ColumnMatrix<GLdouble> knots_vector) {
        return knots_vector;
    }

    TrigoBernsteinSurfPopulation::Individual* TrigoBernsteinSurfPopulation::selection(RowMatrix<Individual*> _individuals) {
        GLuint index = rand() % _number_of_individuals;
        return _individuals[index];
    }

    GLboolean TrigoBernsteinSurfPopulation::mutation() {
        Individual* actual = selection(_individuals);

        // [1 ..   knot_nr - 2] <- because indexes are between [0 .. knor_nr -1] but first and last we doesn`t want to change
        int i = rand() % (actual->_u_knot_vector.GetColumnCount() - 2) + 1;
        int j = rand() % (actual->_v_knot_vector.GetRowCount() - 2) + 1;

        GLdouble v;
        GLdouble scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);     // [0 ..   1]


        while (scale > r_max) {                                 // force [0 .. r_max] interval
            scale = ((GLdouble)rand() / (GLdouble)RAND_MAX);    // [0 ..   1]
        }

        GLdouble udiff;
        GLdouble vdiff;
        GLdouble old_length = actual->getFitness();
        // u_knot (i)
        {
            v = (GLdouble)rand() / (GLdouble)RAND_MAX;           // [0 ..   1]
            if (v < 0.5) {
                // i-1 and i : diff negative
                udiff = actual->_u_knot_vector[i-1] - actual->_u_knot_vector[i];
            } else {
                // i and i+1 : diff positiv
                udiff = actual->_u_knot_vector[i+1] - actual->_u_knot_vector[i];
            }
            actual->_u_knot_vector[i] += (udiff) * scale;
        }
        // v_knot (j)
        {
            v = (GLdouble)rand() / (GLdouble)RAND_MAX;           // [0 ..   1]
            if (v < 0.5) {
                // i and i-1
                vdiff = actual->_v_knot_vector[j-1] - actual->_v_knot_vector[j];
            } else {
                // i and i+1
                vdiff = actual->_v_knot_vector[j+1] - actual->_v_knot_vector[j];
            }
            actual->_v_knot_vector[j] += (vdiff) * scale;
        }

        actual->calculateInterpolation();
        if (actual->getFitness() > old_length) {
            actual->_u_knot_vector(i) -= (udiff) * scale;
            actual->_v_knot_vector(j) -= (vdiff) * scale;
            actual->calculateInterpolation();

            return GL_FALSE;
        }

        sort();
//        for (GLuint i = 0; i < _number_of_individuals; i++) {
//            cout << "Area of [" << i << "] = " <<  _individuals[i]->getFitness() << endl;
//        }

        return GL_TRUE;
    }

    GLboolean TrigoBernsteinSurfPopulation::recombination() {
        Individual* parent1 = selection(_individuals);
        Individual* parent2 = selection(_individuals);
//        cout << "parent1 = " << parent1->getFitness() << endl;
//        cout << parent1->_u_knot_vector;
//        cout << "parent2 = " << parent2->getFitness() << endl;
//        cout << parent2->_u_knot_vector;


        GLuint u_i = rand() % (parent1->_u_knot_vector.GetColumnCount() - 2) + 1;
        GLuint v_i = rand() % (parent1->_v_knot_vector.GetRowCount() - 2) + 1;


        RowMatrix<GLdouble> child1_u;
        ColumnMatrix<GLdouble> child1_v;
        RowMatrix<GLdouble> child2_u;
        ColumnMatrix<GLdouble> child2_v;

        child1_u.ResizeColumns(parent1->_u_knot_vector.GetColumnCount());
        child2_u.ResizeColumns(parent1->_u_knot_vector.GetColumnCount());

        child1_v.ResizeRows(parent1->_v_knot_vector.GetRowCount());
        child2_v.ResizeRows(parent1->_v_knot_vector.GetRowCount());

        for (GLuint i = 0; i < u_i; i++) {
            child1_u[i] = parent1->_u_knot_vector[i];
            child2_u[i] = parent2->_u_knot_vector[i];
        }

        for (GLuint i = u_i; i < parent1->_u_knot_vector.GetColumnCount(); i++) {
            child1_u[i] = parent2->_u_knot_vector[i];
            child2_u[i] = parent1->_u_knot_vector[i];
        }

//        cout << child1_u;
//        cout << child2_u;

        for (GLuint i = 0; i < v_i; i++) {
            child1_v[i] = parent1->_v_knot_vector[i];
            child2_v[i] = parent2->_v_knot_vector[i];
        }

        for (GLuint i = v_i; i < parent1->_v_knot_vector.GetRowCount(); i++) {
            child1_v[i] = parent2->_v_knot_vector[i];
            child2_v[i] = parent1->_v_knot_vector[i];
        }

        Individual* child1 = new Individual(_alpha,_n,_beta,_m,_data_net_to_interpolate,child1_u,child1_v);
        Individual* child2 = new Individual(_alpha,_n,_beta,_m,_data_net_to_interpolate,child2_u,child2_v);

        child1->calculateInterpolation();
        child1->calculateLength();

        child2->calculateInterpolation();
        child2->calculateLength();

        searchPlace(parent1, parent2, child1);
        sort();
        searchPlace(parent1, parent2, child2);
        sort();

        return GL_TRUE;
    }

    GLvoid TrigoBernsteinSurfPopulation::searchPlace(Individual* first_parent, Individual* second_parent, Individual* child) {
        if (child->getFitness() < first_parent->getFitness()) {
//            delete first_parent, first_parent = 0;
            first_parent = child;
        } else {
            if (child->getFitness() < second_parent->getFitness()) {
//                delete second_parent, second_parent = 0;
                second_parent = child;
            } else {
                if (child->getFitness() < _individuals[_individuals.GetColumnCount()-2]->getFitness()) {
//                    delete _individuals[_individuals.GetColumnCount()-2], _individuals[_individuals.GetColumnCount()-2] = 0;
                    _individuals[_individuals.GetColumnCount()-2] = child;
                } else {
                    if (child->getFitness() < _individuals[_individuals.GetColumnCount()-1]->getFitness()) {
//                        delete _individuals[_individuals.GetColumnCount()-1], _individuals[_individuals.GetColumnCount()-1] = 0;
                        _individuals[_individuals.GetColumnCount()-1] = child;
                    }
                }
            }
        }
    }

    GLvoid TrigoBernsteinSurfPopulation::evolve() {
        GLdouble mutation_nr = 5.0;
        for (GLuint i = 0; i < _iterationBetweenGeneration; i++) {
            GLdouble r = (GLdouble)rand() / RAND_MAX;
            if (r <= mutation_nr) {
                mutation();
            } else {
                recombination();
            }
        }
        getBest()->render();
    }
}
